#!/usr/bin/env python3
import rclpy
from rclpy.node import Node
from std_msgs.msg import String
import time
import numpy as np
from rclpy.qos import QoSProfile, QoSReliabilityPolicy, QoSDurabilityPolicy

class ROS2LatencySubscriber(Node):
    def __init__(self):
        super().__init__('ros2_latency_subscriber')
        qos = QoSProfile(
            reliability=QoSReliabilityPolicy.RELIABLE,  # 修改为 RELIABLE
            durability=QoSDurabilityPolicy.VOLATILE,
            depth=10
        )
        self.subscription = self.create_subscription(
            String, 'latency_topic', self.callback, qos)
        self.latencies = []

    def callback(self, msg):
        receive_time = time.time_ns()
        send_time = int(msg.data)
        self.latencies.append(receive_time - send_time)

        if len(self.latencies) % 1000 == 0:
            avg = np.mean(self.latencies[-1000:]) / 1e6
            max_latency = np.max(self.latencies[-1000:]) / 1e6
            print(f"ROS2 Latency: Avg={avg:.3f}ms, Max={max_latency:.3f}ms")

def main():
    rclpy.init()
    node = ROS2LatencySubscriber()
    rclpy.spin(node)
    node.destroy_node()
    rclpy.shutdown()

if __name__ == '__main__':
    main()
